function [qWrapped] = quatwrap(q) %#codegen
%QUATWRAP 将四元数转角规整到[-π, π]区间（使标量部分非负）
%
% Input Arguments:
% # q: m*4矩阵，四元数
%
% Output Arguments:
% # qWrapped: m*4矩阵，若q中对应四元数标量为负数，则整体取负数，否则保持一致
%
% References
% # 《应用导航算法工程基础》“特征四元数的性质”

qWrapped = q;
negInd = q(:, 1)<0;
qWrapped(negInd, :) = -q(negInd, :);
end